
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    Potentiometer,  sensorPotentiometer)
#pragma config(Sensor, I2C_1,  frontIEM,       sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  rightIEM,       sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_3,  backIEM,        sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_4,  leftIEM,        sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port2,           thing1top,     tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port3,           wristmotorr,   tmotorVex393, openLoop)
#pragma config(Motor,  port4,           armMotor,      tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port5,           armMotor2,     tmotorVex393, openLoop)
#pragma config(Motor,  port6,           wristmotorl,   tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port7,           frontMotor,    tmotorVex269, openLoop, encoder, encoderPort, I2C_1, 1000)
#pragma config(Motor,  port8,           rightMotor,    tmotorVex269, openLoop, encoder, encoderPort, I2C_4, 1000)
#pragma config(Motor,  port9,           leftMotor,     tmotorVex269, openLoop, reversed, encoder, encoderPort, I2C_2, 1000)
#pragma config(Motor,  port10,          backMotor,     tmotorVex269, openLoop, reversed, encoder, encoderPort, I2C_3, 1000)

//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
void clearsensors()
{
	nMotorEncoder[rightMotor] = 0;
	nMotorEncoder[leftMotor] = 0;
	nMotorEncoder[frontMotor] = 0;
	nMotorEncoder[backMotor] = 0;
}
void drive(int time, bool forward)
{
	int speed = 100;
	if(forward == false)
	{
		speed = -100;
	}
	motor(leftMotor)= speed;
	motor(rightMotor)= speed;
	wait1Msec(time);
	motor(leftMotor)= 0;
	motor(rightMotor)= 0;
}
void suck(int time, bool out)
{
	if (!out)
	{
		motor(thing1top)= -127;
	}
	else
	{
		motor(thing1top)= 127;
	}
	wait1Msec(time);
	motor(thing1top)= 0;
}
void suckIn(int time)
{
	suck(time,false);
}
void suckOut(int time)
{
	suck(time,true);
}
void turn(int degrees)
{
	float encoderValuePerDegree = 5.2;
	int target = encoderValuePerDegree * degrees;
	clearsensors();
	motor(frontMotor)= 100;
	motor(backMotor)= 100;
	motor(leftMotor)= 100;
	motor(rightMotor)= 100;
	while (SensorValue(backIEM)< target)
	{
		//wait
	}
	motor(frontMotor)= 0;
	motor(backMotor)= 0;
	motor(leftMotor)= 0;
	motor(rightMotor)= 0;
}
void arm(bool up)
{
	if(!up)
	{
		motor(armMotor)= 100;
		motor(armMotor2)= 100;
		wait1Msec(500);
		motor(armMotor)= 0;
		motor(armMotor2)= 0;
	}
	else
	{
		motor(armMotor)= 90;
		motor(armMotor2)= 90;
	}
}
void armUp()
{
	arm(true);
}
void armDown()
{
	arm(false);
}
void wrist(bool up)
{
	if(!up)
	{
		motor(wristmotorl)= 100;
		motor(wristmotorr)= 100;
		wait1Msec(500);
		motor(wristmotorl)= 0;
		motor(wristmotorr)= 0;
	}
	else
	{
		motor(wristmotorl)= 100;
		motor(wristmotorr)= 100;
	}
}
void wristUp()
{
	wrist(true);
}
void wristDown()
{
	wrist(false);
}
/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{

	bStopTasksBetweenModes = true;
	nMotorEncoder[rightMotor] = 0;
	nMotorEncoder[leftMotor] = 0;
	nMotorEncoder[frontMotor] = 0;
	nMotorEncoder[backMotor] = 0;
}


/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
	//bMotorReflected[port8] = true;
	//bMotorReflected[port9] = true;
	//bMotorReflected[port10] = true;
	//bMotorReflected[port7] = false;
	//wait1Msec(1000);
	//motor(thing1top) = 127;
	//wait1Msec(1000);
	//motor(thing1top) = 0;
	//wait1Msec(1000);
	drive(1500, true);
	suckIn(1000);
	turn(45);
	wristUp();
	armUp();

	//motor(leftMotor) = 100;
	//motor(rightMotor) = 100;
	//wait1Msec(2000);
	//motor(leftMotor) = 0;
	//motor(rightMotor) = 0;
	//motor(thing1top) = 100;
	//wait1Msec(2000);
	//motor(thing1top) = 0;
	//motor(leftMotor)= 100;
	//motor(rightMotor) = 100;
	//wait1Msec(1000);
	//motor(wristmotorl) = 100;
	//motor(wristmotorr) = 100;
	//wait1Msec(500);
	//motor(armMotor) = 100;
	//motor(armMotor2) = 100;
	//motor(wristmotorl) = 100;
	//motor(wristmotorr) = 100;
	//wait1Msec(2000);
	//motor(thing1top) = -127;
	//wait1Msec(750);
	//motor(armMotor) = -100;
	//motor(armMotor2) = -100;
	//motor(wristmotorl) = 0;
	//motor(wristmotorr) = 0;
	//wait1Msec(1000);
	//motor(armMotor) = 0;
	//motor(armMotor2) = 0;
	//motor(leftMotor) = -100;
	//motor(rightMotor) = -100;
	//wait1Msec(3000);
	//motor(leftMotor) = 0;
	//motor(rightMotor) = 0;
}
//forward,stop,scoop up yellow, small left turn, forward, lift arm and wrist, turn grabber.
/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	while (true)
	{
		if(vexRT[Btn6U]==1)
		{
			motor[thing1top] = -127;
		}
		else if (vexRT[Btn6D] == 1)
		{
			motor[thing1top] = 127;
		}
		else
		{
			motor[thing1top] = 0;
		}

		if( vexRT[Ch4] == 0)
		{
			int iCh1 = vexRT[Ch1];
			int iCh2 = vexRT[Ch2];
			if(vexRT[Ch1] > -15 && vexRT[Ch1] < 15)
			{

				iCh1 = 0;
			}

			bMotorReflected[port8] = true;
			bMotorReflected[port9] = true;
			bMotorReflected[port10] = true;
			bMotorReflected[port7] = false;
			motor[port10] = iCh1;							// Remove these as well and replace with your appropriate code
			motor[port7] = iCh1;              // Remove these as well and replace with your appropriate code

			motor[port8] = iCh2;
			motor[port9] = iCh2;
			motor[armMotor] = vexRT[Ch3Xmtr2];
			motor[armMotor2] = vexRT[Ch3Xmtr2];
			motor[wristmotorl] = vexRT[Ch2Xmtr2];
			motor[wristmotorr] = vexRT[Ch2Xmtr2];
		}
		else
		{
			bMotorReflected[port8] = false;
			bMotorReflected[port9] = true;
			bMotorReflected[port10] = false;
			bMotorReflected[port7] = false;
			motor[port8] = vexRT[Ch4];							// Remove these as well and replace with your appropriate code
			motor[port9] = vexRT[Ch4];              // Remove these as well and replace with your appropriate code
			motor[port10] = vexRT[Ch4];
			motor[port7] = vexRT[Ch4];
			motor[armMotor] = vexRT[Ch3Xmtr2];
			motor[armMotor2] = vexRT[Ch3Xmtr2];
			motor[wristmotorl] = vexRT[Ch2Xmtr2];
			motor[wristmotorr] = vexRT[Ch2Xmtr2];
		}
	}
}
